﻿Imports System.IO

Public Enum Alliance As Byte
    Red = 82
    Blue = 66
End Enum

Public Class DSPacket
    Implements ICloneable

    Public Sub New(ByVal packetIndex As Integer)
        Me.PacketId = packetIndex
        ControlData = New RobotControlData()
        DsInputs = New DsInputs(0)
        Dim sticks(3) As Joystick
        For i = 0 To 3
            sticks(i) = New Joystick(i)
        Next
        Joysticks = sticks
        Version = New DSVersion()
        Dim inputs(3) As UInt16
        AnalogInputs = inputs
    End Sub

    Private _packetId As UInt16
    Private _controlData As RobotControlData
    Private _dsInputs As DsInputs
    Private _teamNumber As Integer
    Private _alliance As Alliance = Dashboard.Alliance.Red
    Private _position As Byte = 2

    Private _joysticks As Joystick()
    Private _analogInputs As UInt16()

    Private _cRioChecksum As UInt64 = 3472328296227680304
    Private _fpgaChecksum0 As UInt32 = 1179010630
    Private _fpgaChecksum1 As UInt32 = 1179010630
    Private _fpgaChecksum2 As UInt32 = 1179010630
    Private _fpgaChecksum3 As UInt32 = 1179010630

    Private Dim _version As DSVersion

    Public Function Clone() As Object Implements System.ICloneable.Clone
        Dim out As New DSPacket(Me.PacketId)
        out.Alliance = Alliance
        out.AnalogInputs = AnalogInputs
        out.ControlData = ControlData
        out.CRioChecksum = CRioChecksum
        out.DsInputs = DsInputs
        out.FpgaChecksum0 = FpgaChecksum0
        out.FpgaChecksum1 = FpgaChecksum1
        out.FpgaChecksum2 = FpgaChecksum2
        out.FpgaChecksum3 = FpgaChecksum3
        out.Joysticks = Joysticks.Clone()
        out.PacketId = PacketId
        out.Position = Position
        out.TeamNumber = TeamNumber
        out.Version = Version
        Return out
    End Function

    Public Function GetNextPacket()
        Dim r As DSPacket = Me.Clone()
        r.PacketId += 1
        Return r
    End Function

    Public Function GetBytes() As Byte()
        Try
            Dim stream As MemoryStream = New MemoryStream(1024)
            Dim writer As New BinaryWriter(stream)

            writer.EWrite(CUShort(PacketId))

            writer.Write(ControlData.Getbytes())
            writer.Write(DsInputs.GetByte())
            writer.WriteTeamNumber(TeamNumber)
            writer.Write(CByte(Alliance))
            writer.Write(CByte(48 + Position))
            For Each joystick In Joysticks
                Dim joystickData As Byte() = joystick.GetBytes()
                writer.Write(joystickData)
            Next
            For Each input As UInt16 In AnalogInputs
                writer.EWrite(input)
            Next
            writer.EWrite(CRioChecksum)
            writer.EWrite(FpgaChecksum0)
            writer.EWrite(FpgaChecksum1)
            writer.EWrite(FpgaChecksum2)
            writer.EWrite(FpgaChecksum3)

            writer.Write(Version.GetBytes())

            Dim paddingLength = 1024
            paddingLength -= writer.BaseStream.Position
            Dim padding(paddingLength - 1) As Byte
            writer.Write(padding)
            'TODO: Figure out why CRC calc is not being accepted on robot.
            Dim crcData = stream.ToArray()
            stream.Position -= 4
            writer.Write(BitConverter.ToUInt32((New Crc32).ComputeHash(crcData), 0))

            Dim data = stream.ToArray()
            writer.Close()

            Debug.Assert(VerifyFrcCrc(data))

            Return data
        Catch ex As Exception
            Throw
        End Try
    End Function
    Public Property Alliance() As Alliance
        Get
            Return _alliance
        End Get
        Set(ByVal value As Alliance)
            _alliance = value
        End Set
    End Property
    Public Property AnalogInputs() As UInt16()
        Get
            Return _analogInputs
        End Get
        Set(ByVal value As UInt16())
            _analogInputs = value
        End Set
    End Property
    Public Property ControlData() As RobotControlData
        Get
            Return _controlData
        End Get
        Set(ByVal value As RobotControlData)
            _controlData = value
        End Set
    End Property
    Public Property CRioChecksum() As UInt64
        Get
            Return _cRioChecksum
        End Get
        Set(ByVal value As UInt64)
            _cRioChecksum = value
        End Set
    End Property
    Public Property DsInputs() As DsInputs
        Get
            Return _dsInputs
        End Get
        Set(ByVal value As DsInputs)
            _dsInputs = value
        End Set
    End Property
    Public Property FpgaChecksum0() As UInt32
        Get
            Return _fpgaChecksum0
        End Get
        Set(ByVal value As UInt32)
            _fpgaChecksum0 = value
        End Set
    End Property
    Public Property FpgaChecksum1() As UInt32
        Get
            Return _fpgaChecksum1
        End Get
        Set(ByVal value As UInt32)
            _fpgaChecksum1 = value
        End Set
    End Property
    Public Property FpgaChecksum2() As UInt32
        Get
            Return _fpgaChecksum2
        End Get
        Set(ByVal value As UInt32)
            _fpgaChecksum2 = value
        End Set
    End Property
    Public Property FpgaChecksum3() As UInt32
        Get
            Return _fpgaChecksum3
        End Get
        Set(ByVal value As UInt32)
            _fpgaChecksum3 = value
        End Set
    End Property
    Public Property Joysticks() As Joystick()
        Get
            Return _joysticks
        End Get
        Set(ByVal value As Joystick())
            _joysticks = value
        End Set
    End Property
    Public Property PacketId() As Integer
        Get
            Return _packetId
        End Get
        Set(ByVal value As Integer)
            _packetId = value
        End Set
    End Property
    Public Property Position() As Byte
        Get
            Return _position
        End Get
        Set(ByVal value As Byte)
            _position = value
        End Set
    End Property
    Public Property TeamNumber() As Integer
        Get
            Return _teamNumber
        End Get
        Set(ByVal value As Integer)
            _teamNumber = value
        End Set
    End Property
    Public Property Version() As DSVersion
        Get
            Return _version
        End Get
        Set(ByVal value As DSVersion)
            _version = value
        End Set
    End Property
End Class
